/*
  File: driver.cpp
  Original author: HANGZHOU OLE-SYSTEMS
  Date of creation: 
  Modifier: TAN Hongkui 谭洪奎
  Email of modifier: tanhongkui@qq.com
  Date of modification: September 8, 2023
  Description: This file contains class Driver that can connect to 2D LiDAR of Ole and read packets.
*/

#include <arpa/inet.h>
#include <errno.h>
#include <fcntl.h>
#include <netinet/in.h>
#include <poll.h>
#include <sys/file.h>
#include <sys/socket.h>
#include <unistd.h>
#include <cmath>
#include <functional>
#include <thread>
#include <filesystem>

#include <spdlog/spdlog.h>
#include <yaml-cpp/yaml.h>
#include <simple/BinaryInterfaceServer.h>
#include <simple/LaserMessage.h>

#include "constants.h"

// #include <diagnostic_updater/diagnostic_updater.h>
// #include <diagnostic_updater/publisher.h>
#include <oleiPacket.h>
// #include <oleiScan.h>
#include <iostream>

// here maskoff waring of macro 'ROS_LOG'
#pragma clang diagnostic ignored "-Wzero-as-null-pointer-constant"

namespace olelidar
{

  using namespace olei_msgs;
  // using namespace diagnostic_updater;

  /// Constants
  static constexpr uint16_t kUdpPort = 2368;
  static constexpr size_t kPacketSize = sizeof(oleiPacket().data);
  static constexpr int kError = -1;

  class Driver
  {
    using data_cb_t = std::function<void(const oleiPacketConstPtr &)>;

  public:
    // explicit Driver(const ros::NodeHandle &pnh);
    explicit Driver(std::filesystem::path config_path);
    ~Driver();

    bool Poll();

    void setCallback(const data_cb_t &cb) { data_cb_ = cb; }

  private:
    bool OpenUdpPort();
    int ReadPacket(oleiPacket &packet);

    // Ethernet relate variables
    std::string device_ip_str_;
    std::string local_ip_str_;
    std::string multiaddr_ip_str_;
    int device_port_;
    in_addr device_ip_;
    int socket_id_{-1};
    std::chrono::steady_clock::time_point last_time_;

    // load the YAML configuration file
    YAML::Node config_;

    // Diagnostics updater
    // diagnostic_updater::Updater updater_;
    // boost::shared_ptr<diagnostic_updater::TopicDiagnostic> topic_diag_;
    std::vector<oleiPacket> buffer_;
    // double freq_;

    // raw data callback
    data_cb_t data_cb_{nullptr};
    std::thread data_thr_;
    bool is_loop_{false};
  };

  // Driver::Driver(const ros::NodeHandle &pnh)
  Driver::Driver(std::filesystem::path config_path)
      : last_time_(std::chrono::steady_clock::now())
  {
    config_ = YAML::LoadFile(config_path);
    spdlog::info("packet size: {}", kPacketSize);
    device_ip_str_ = config_["device_ip"].as<std::string>();
    device_port_ = config_["device_port"].as<int>();
    local_ip_str_ = config_["local_ip"].as<std::string>();
    multiaddr_ip_str_ = config_["multiaddr"].as<std::string>();
    // simple_port_ = config_["simple_port"].as<int>();

    spdlog::info("device_ip: {}:{}", device_ip_str_.c_str(), device_port_);

    if (inet_aton(device_ip_str_.c_str(), &device_ip_) == 0)
    {
      // inet_aton() returns nonzero if the address is valid, zero if not.
      spdlog::critical("Invalid device ip: {}:{}", device_ip_str_.c_str(), device_port_);
      // ros::shutdown();
      std::exit(1);
    }

    // Output
    // pub_packet_ = pnh_.advertise<oleiPacket>("packet", 10);

    if (!OpenUdpPort())
      spdlog::error("Failed to open UDP Port");

    data_thr_ = std::move(std::thread([&]
                                      {
      is_loop_ = true;
      while (is_loop_) {
          if(!is_loop_) break; 
          Poll();
      } }));
    spdlog::info("Successfully init olelidar driver");
  }

  Driver::~Driver()
  {
    is_loop_ = false;
    if (data_thr_.joinable())
    {
      data_thr_.join();
    }
    if (close(socket_id_))
    {
      spdlog::info("Close socket {} at {}", socket_id_, device_ip_str_.c_str());
    }
    else
    {
      spdlog::error("Failed to close socket {} at {}", socket_id_, device_ip_str_.c_str());
    }
  }

  bool Driver::OpenUdpPort()
  {
    socket_id_ = socket(AF_INET, SOCK_DGRAM, 0);
    if (socket_id_ == -1)
    {
      perror("socket");
      spdlog::error("Failed to create socket");
      return false;
    }

    sockaddr_in my_addr;                              // my address information
    memset(&my_addr, 0, sizeof(my_addr));             // initialize to zeros
    my_addr.sin_family = AF_INET;                     // host byte order
    my_addr.sin_port = htons(uint16_t(device_port_)); // short, in network byte order
    my_addr.sin_addr.s_addr = INADDR_ANY;             // automatically fill in my IP

    if (bind(socket_id_, (sockaddr *)&my_addr, sizeof(sockaddr)) == -1)
    {
      perror("bind"); // TODO: spdlog::error errno
      spdlog::error("Failed to bind to socket {}", socket_id_);
      return false;
    }
    if (multiaddr_ip_str_ != "")
    {
      // 加入组播才能接受到组播信息
      struct ip_mreq mreq;
      mreq.imr_multiaddr.s_addr = inet_addr(multiaddr_ip_str_.c_str());
      mreq.imr_interface.s_addr = inet_addr(local_ip_str_.c_str());
      int err = setsockopt(socket_id_, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq));
      spdlog::info("AddMultiaddr:{}  local:{}   =>{} ", multiaddr_ip_str_.c_str(), local_ip_str_.c_str(), err);
    }
    if (fcntl(socket_id_, F_SETFL, O_NONBLOCK | FASYNC) < 0)
    {
      perror("non-block");
      spdlog::error("Failed to set socket to non-blocking");
      return false;
    }
    return true;
  }

  int Driver::ReadPacket(oleiPacket &packet)
  {
    // ros::Time time_before = ros::Time::now();
    std::chrono::steady_clock::time_point time_before = std::chrono::steady_clock::now();

    struct pollfd fds[1];
    fds[0].fd = socket_id_;
    fds[0].events = POLLIN;
    const int timeout_ms = 500; // one second (in msec)

    sockaddr_in sender_address;
    socklen_t sender_address_len = sizeof(sender_address);

    while (true)
    {
      do
      {
        const int retval = poll(fds, 1, timeout_ms);
        if (retval < 0)
        {
          if (errno != EINTR)
            spdlog::error("poll() error: {}", strerror(errno));
          return kError;
        }
        else if (retval == 0)
        {
          spdlog::warn("oleilidar poll() timeout");
          return kError;
        }

        if ((fds[0].revents & POLLERR) || (fds[0].revents & POLLHUP) || (fds[0].revents & POLLNVAL))
        {
          spdlog::error("poll() reports oleilidar error");
          return kError;
        }
      } while ((fds[0].revents & POLLIN) == 0);

      // Receive packets that should now be available from the
      // socket using a blocking read.
      const ssize_t nbytes = recvfrom(socket_id_, &packet.data[0], kPacketSize, 0, (sockaddr *)&sender_address, &sender_address_len);

      if (nbytes < 0)
      {
        if (errno != EWOULDBLOCK)
        {
          perror("recvfail");
          spdlog::error("Failed to read from socket");
          return kError;
        }
      }
      else if (static_cast<size_t>(nbytes) == kPacketSize)
      {
        // else if ((size_t)nbytes == kPacketSize) {
        //  read successful,
        //  if packet is not from the lidar scanner we selected by IP,
        //  continue otherwise we are done
        // spdlog::error("Failed to read from socket");

        // spdlog::info("sender:{}   device:{}",sender_address.sin_addr.s_addr,device_ip_.s_addr);
        if (device_ip_str_ != "" && sender_address.sin_addr.s_addr != device_ip_.s_addr)
          continue;
        else
          break; // done
      }

      // ROS_DEBUG_STREAM("incomplete olei packet read: " << nbytes << " bytes");
      spdlog::debug("incomplete olei packet read: {} bytes", nbytes);
    }

    packet.stamp = time_before;

    // TODO
    // #ifdef TIMESTAMP_DEBUG
    //     ros::Duration delta = time_before - last_time_;
    //     ROS_INFO_STREAM("raw data delta time: " << time_before << "," << delta.toSec() * 1000);
    //     last_time_ = time_before;
    // #endif

    return 0;
  }

  bool Driver::Poll()
  {
    oleiPacket::Ptr packet(new oleiPacket);

    while (true)
    {
      // keep reading until full packet received
      const int rc = ReadPacket(*packet);
      if (rc == 0)
        break; // got a full packet?
      if (rc < 0)
        return false; // end of file reached?
    }

    // publish message using time of last packet read

    // TODO
    // #ifdef DRIVER_MODULE
    //     pub_packet_.publish(packet);
    // #else
    data_cb_(packet);
    // #endif

    return true;
  }

} // namespace olelidar

// int main(int argc, char **argv)
// {
//   try
//   {
//     // BinaryInterfaceServer server(simple_port);
//     olelidar::Driver node;

//     while (true)
//     {
//       // poll device until end of file
//       if (!node.Poll())
//       {
//         spdlog::warn("Failed to poll device");
//         // break;
//       }
//       // ScanData data = driver_hdr->GetFullScan();
//       // Poco::Buffer<char> laser_datagram = ConvertLaserMsg2Datagram(data, start_angle, end_angle, frame_name);
//       // server.write(laser_datagram.begin(), laser_datagram.size());
//       // sleep(1 / 60);
//     }
//   }
//   catch (std::exception &ex)
//   {
//     std::cerr << "TestClientLocalizationPose died with error!" << std::endl;
//     std::cerr << "What: " << std::string(ex.what()) << std::endl;
//   }
// }

// #ifdef DRIVER_MODULE
// int main(int argc, char **argv)
// {
//   ros::init(argc, argv, "olelidar_driver");
//   ros::NodeHandle pnh("~");

//   olelidar::Driver node(pnh);

//   while (ros::ok())
//   {
//     // poll device until end of file
//     if (!node.Poll())
//     {
//       spdlog::warn("Failed to poll device");
//       // break;
//     }
//     ros::spinOnce();
//   }
//   return 0;
// }
// #endif
